#ifndef MLTOOL_CHOLESKY_H
#define MLTOOL_CHOLESKY_H

#include"tags.h"
#include"triangular_solver.h"

namespace math{

/*
  Input :
    Mat ---- the type of the matrix to be factorized. It should be a
             symmetric_matrix<T, upper>.
      A ---- the matrix to be factorized.
  Output :
    The function outputs an integer singular_ to indicate whether A has been
    factorized successfully.
    Singular_ being not equal to 0 means that A is singular and the factorizing
    processing is failed.
  After:
    if it returns 0, A is factorized to L * L^T, where L is a lower triangular matrix.
    A is overwritten by L^T.
    If it returns non-zero, A is messed up.
  */
template<class Sym_Mat>
int cholesky_factorize(Sym_Mat& A, upper_tag)
{
    typedef typename Sym_Mat::value_type T;
    typedef Sym_Mat::size_type size_type;
    size_type  sz = A.size1();
    size_type  singular_ = 0;

    for(size_type  i = 0; i < sz; ++i)
    {
        for(size_type  j = 0; j < i; ++j)
            A(i,i) -= A(j,i)*A(j,i);
        if(A(i,i) > T())
        {
            A(i,i) = std::sqrt(A(i,i));
            for(size_type  j = i + 1; j < sz; ++j)
            {
                for(size_type  k = 0; k < i; ++k)
                {
                    A(i,j) -= A(k,i)*A(k,j);

                }
                A(i,j) /= A(i,i);
            }
        }
        else if(singular_ == 0)
        {
            singular_ = i + 1;
            return singular_;
        }
    }
    return singular_;
}

template<class Sym_Mat>
int  cholesky_factorize(Sym_Mat & A,lower_tag)
{
    typedef typename Sym_Mat::value_type T;
    typename Sym_Mat::size_type size_type;
    size_type  sz = A.size1();
    size_type  singular_ = 0;

    for(size_type  i = 0; i < sz; ++i)
    {
        for(size_type  j = 0; j < i; ++j)
            A(i,i) -= A(i,j)*A(i,j);
        if(A(i,i) > T())
        {
            A(i,i) = std::sqrt(A(i,i));
            for(size_type  j = i + 1; j < sz; ++j)
            {
                for(size_type  k = 0; k < i; ++k)
                {
                    A(j,i) -= A(i,k)*A(j,k);

                }
                A(j,i) /= A(i,i);
            }
        }
        else if(singular_ == 0)
        {
            singular_ = i + 1;
            return singular_;
        }
    }
    return singular_;
}

// dispatch according to H's triangular_category.
template<class Sym_Mat>
int  cholesky_factorize(Sym_Mat & H)
{
    typedef typename Sym_Mat::triangular_category triangular_category;
    return cholesky_factorize(H, triangular_category());
}

// solve Hx = b
template<class Sym_Matrix, class Right_Vec>
int cholesky_inplace_solve(Sym_Matrix& H, Right_Vec &b)
{
    int singular = cholesky_factorize(H);
    if(singular != 0)
    {
        std::cerr << "Coefficient matrix is singular!" << std::endl;
    }
    else
    {
        inplace_solve(H, b, lower_tag);
        inplace_solve(H, b, upper_tag);
    }
    return singular;
}

}
#endif
